#include "xd_message.h"
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include "motor_def.h"
#include "motor.h"
#include "io.h"
#include "adc.h"
#include "gqueue.h"
#include "zy_tools.h"
#include "ffb_config.h"
#include "grb_control.h"
#include "ver.h"
#define SEND_MAX_LEN        128         // 注意此长度不能超过   CFG_TUD_CDC_TX_BUFSIZE

extern gqueue_t qAbsMove;
static uint16_t(*send)(void const*,uint32_t);       // 发送数据的函数

void xd_message_set_send_func(uint16_t(*func)(void const*,uint32_t))
{
    send =  func;
}

int xd_message_pack_a5(uint8_t type,uint8_t* data,int len,uint8_t* dstBuf)
{
    if(dstBuf == NULL)      // data == NULL 是没有数据
    {
        return -1;
    }
    uint16_t crc16 = 0;
    dstBuf[0] = 0xAA;
    dstBuf[1] = 0x55;
    dstBuf[2] = len;
    dstBuf[3] = type;
    if(len > 0)
    {
        memcpy(dstBuf+4,data,len);
    }
    crc16 = CRC16(dstBuf,len+4);
    dstBuf[len+4] = crc16&0xFF;
    dstBuf[len+5] = (crc16>>8)&0xFF;
    return 0;
}

bool send_message(uint8_t type,uint8_t* data,int len)
{
    uint8_t send_buf[SEND_MAX_LEN] = {0};
    if(len > SEND_MAX_LEN - HEADER_LENGTH)     // 包超长
    {
        return false;
    }
    if(!xd_message_pack_a5(type,data,len,send_buf))
    {
        send(send_buf,len+HEADER_LENGTH);
        return true;
    }
    return false;
}


void On_Xd_Cycle_Up(void)
{
    card_cycle_up_t xdu;
    memset(&xdu,0,sizeof(xdu));
    for(int i=1;i<TOTAL_MOTORS;i++)     // 第一个电机是力反馈电机，不需要获取位置
    {
        uint8_t nodeId = i+1;
        xdu.actual[i] = Canopen_Motor_Get_Pos(nodeId);
    }
    // AI、DI在hid里发送
//    Din_Get(xdu.din);
//    for(int i=0;i<JOY_AXIS_NUM;i++)
//    {
//        xdu.ain[i] = Joystick_Get_ScaleValue(i);
//    }

    send_message(CARD_CYCLE_UP,(uint8_t*)&xdu,sizeof(xdu));

}
void Motor_AbsMove_Rcv(void);
void OnRcv_Xd_Cycle_Down(uint8_t* rcvData)
{
    xd_cycle_down_t xdd;
    if(rcvData == NULL){
        return;
    }
    memcpy(&xdd,rcvData,sizeof(xdd));
    // 电机目标位置部分
    if(xdd.enBits & 0x01)
    {
        abs_move_buf_t buf;
        for(int i=1;i<TOTAL_MOTORS;i++)     // 第一个电机不需要处理
        {
            buf.nodeId = i+1;
            buf.pMotor = Canopen_Motor_Get(buf.nodeId);
            if(buf.pMotor)
            {
                if(buf.pMotor->homeStatus > BHS_SUCCESS){       // 只有在idle和success的情况下才允许动作。
                    continue;
                }
                buf.target = xdd.target[i];
                gqueue_push(&qAbsMove,&buf);
                Motor_AbsMove_Rcv();
            }
        }
    }

    // DO输出部分
    Dout_Write(xdd.dout);

//    // PWM输出部分
//    if((xdd.enBits>>2) & 0x01)
//    {
//        for(int i=0; i<PWM_NUMBER; i++)
//        {
//            Pwm_Set(i,500,xdd.pwmout[i]);
//        }
//    }
//    xdDownRcv = true;
    On_Xd_Cycle_Up();
}

extern char HARDVER;
void OnRcv_Xd_Get_Hardver(void)
{
    uint8_t hv[2] = {0};
    hv[0] = Get_HardVer()+0x30;
    send_message(CARD_RE_HARDVER,hv,strlen((const char*)hv));
    return ;
}
void OnRcv_Xd_Get_Softver(void)
{
    uint8_t* ver = (uint8_t*)SOFTVER;
    send_message(CARD_RE_SOFTVER,ver,strlen((const char*)ver));
    return ;
}
void OnRcv_Xd_Set_Homestatus(uint8_t axis)
{
    // axis中每个bit代表一个轴，为1则回零，为0则不处理
    uint8_t _axis = axis;
    uint8_t nodeId = 0;
    for(int i=1;i<TOTAL_MOTORS;i++)
    {
        if((_axis>>i)&0x01)
        {
            nodeId = i+1;
            Canopen_Motor_Home_Start(nodeId);
        }
    }

    send_message(CARD_RE_SET_HOMESTATUS,&_axis,1);
}

void OnRcv_Xd_Get_Homestatus(void)
{
    uint8_t homeStatus = 0,nodeId = 0;;
    abs_move_buf_t buf;

    for(int i=0;i<TOTAL_MOTORS;i++)
    {
        nodeId = i+1;
        buf.pMotor = Canopen_Motor_Get(nodeId);
        if(buf.pMotor)
        {
            if(buf.pMotor->homeStatus == BHS_SUCCESS)
            {
                homeStatus |= (1<<i);
            }
        }
    }
    send_message(CARD_RE_GET_HOMESTATUS,&homeStatus,1);
}

void OnRcv_Xd_Set_Zero(void)
{
    uint8_t nodeId;
    for(int i=1;i<TOTAL_MOTORS;i++)
    {
        nodeId = i+1;
        Canopen_Motor_Set_HomePul(nodeId);     //  两个轴的电机是位置模式
    }
    send_message(CARD_RE_SET_ZERO,NULL,0);
}

void OnRcv_Xd_Get_UID(void)
{
    uint32_t uid[3];
    uid[0] = HAL_GetUIDw0();
    uid[1] = HAL_GetUIDw1();
    uid[2] = HAL_GetUIDw2();
    send_message(CARD_RE_GET_UID,(uint8_t*)uid,sizeof(uid));
}

uint32_t upError[ERROR_NUM];
void OnRcv_Xd_Get_Err(void)
{
    uint32_t upE[ERROR_NUM] = {0};
    uint32_t upEIndex = 0;
    for(int i=0;i<ERROR_NUM;i++)
    {
        if(upError[i]){
            upE[upEIndex++] = upError[i];
        }
    }
    send_message(CARD_UP_ERR,(uint8_t*)upE,upEIndex*4);
}

void Card_Up_Err(uint32_t err)
{
    uint32_t upE = err;
    send_message(CARD_UP_ERR,(uint8_t*)&upE,4);
}

void OnRcv_Xd_Get_Pipe(void)
{
    // 此处不需实现
    uint8_t pipe = 0;
    send_message(CARD_UP_PIPE,(uint8_t*)&pipe,1);
}

void OnRcv_Xd_GRB_Panel_Display(uint8_t* pDisplay)
{
    xd_grb_panel_t display;
    if(pDisplay == NULL){
        return;
    }
    memcpy(&display,pDisplay,sizeof(display));

    grb_panel_control(display);
    send_message(CARD_RE_GRB_PANEL_DISPALY,NULL,0);
}

void OnRcv_Xd_Car_Speed(uint8_t speed)
{
    car_speed_control(speed);
    send_message(CARD_RE_CAR_SPEED,NULL,0);
}


void OnRecvExDeviceControl(uint8_t* str,int strLen)
{
    FFB_Config_Process(str,strLen);
}


void XD_Message_Received(xd_message_t* msg)
{
    uint8_t* pData = msg->data;
    int type = msg->type;
    uint8_t dataLen = msg->data_len;

    switch (type) {

        case XD_CYCLE_DOWN:
            OnRcv_Xd_Cycle_Down(pData);
            break;

        case XD_GET_HARDVER :
            OnRcv_Xd_Get_Hardver();
            break;
        case XD_GET_SOFTVER :
            OnRcv_Xd_Get_Softver();
            break;
        case XD_SET_HOMESTATUS :
            OnRcv_Xd_Set_Homestatus(*pData);
            break;
        case XD_GET_HOMESTATUS :
            OnRcv_Xd_Get_Homestatus();
            break;
        case XD_SET_ZERO :
            OnRcv_Xd_Set_Zero();
            break;
        case XD_GET_UID :
            OnRcv_Xd_Get_UID();
            break;
        case XD_GET_ERR :
            OnRcv_Xd_Get_Err();
            break;
        case XD_GET_PIPE:
            OnRcv_Xd_Get_Pipe();
            break;
        case XD_GRB_PANEL_DISPALY:
            OnRcv_Xd_GRB_Panel_Display(pData);
            break;
        case XD_CAR_SPEED:
            OnRcv_Xd_Car_Speed(*pData);
            break;

        case EX_Device_Control:
            OnRecvExDeviceControl(pData,dataLen);
            break;

        default:
            break;
    }
}



